function [x_comb, P_comb] = track_fusion(x_sens, P_sens, x_prev, P_prev, x_sens_prev, P_sens_prev)
%% Track fusion for 2 sensors but with the view of expanding to more.
%% [NB: if the number of sensors is greater than 2 the result is undefined.]
%% Inputs: 1) x_sens : target state vector for each sensor
%%         2) P_sens : covariance matrix for each sensor
%% Outputs: 1) x_comb : combined target state vector
%%          2) P_comb : combined covariance matrix

%% x_sens_prev - previous (at time of last fusion) sensor state estimates, one per sensor.
%% P_sens_prev - previous (at time of last fusion) sensor covariance matrices, one per sensor.
%% x_prev - previous combined state estimate.
%% P_prev - previous combined covariance matrix.

if (isempty(x_prev) || isempty(P_prev) || isempty(x_sens_prev) || isempty(P_sens_prev))
  
  % Todo: check with Pieter / the reference to see what to do in this case.
  
  %P_c = [P_i^-1 + P_j^-1]^-1;
  P_comb = (P_sens(:,:,1)^-1 + P_sens(:,:,2)^-1)^-1;
  
  %x_c = P_c[P_i^-1 * x_i + P_j^-1 * x_j];
  x_comb = P_comb * (P_sens(:,:,1)^-1 * x_sens(:,1) + P_sens(:,:,2)^-1 * x_sens(:,2));
  
else

  %P_c = [P_i^-1 + P_j^-1 - P_pi^-1 - P_pj^-1 + P_p^-1]^-1;
  P_comb = (P_sens(:,:, 1)^-1 + P_sens(:,:, 2)^-1 - P_sens_prev(:,:, 1)^-1 - P_sens_prev(:,:, 2)^-1 + P_prev^-1)^-1;

  %x_c = P_c[P_i^-1 * x_i + P_j^-1 * x_j - P_pi^-1 * x_pi - P_pj^-1 * x_pj + P_p-1 * x_p];
  x_comb = P_comb * (P_sens(:,:, 1)^-1 * x_sens(:, 1) + P_sens(:,:, 2)^-1 * x_sens(:, 2)... 
            - P_sens_prev(:,:, 1)^-1 * x_sens_prev(:, 1) - P_sens_prev(:,:, 2)^-1 * x_sens_prev(:,2)... 
            + P_prev^-1 * x_prev);
          
end